#!/usr/bin/env python3
import rclpy
from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose, FollowWaypoints
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from threading import Lock

"""处理所有导航动作"""
class NavController:
    def __init__(self, node):
        self.node = node
        self.lock = Lock()
        
        # Action clients
        self.nav_to_pose_client = ActionClient(
            node, NavigateToPose, 'navigate_to_pose')
        self.follow_waypoints_client = ActionClient(
            node, FollowWaypoints, 'follow_waypoints')
        
        # Publishers
        self.initial_pose_pub = node.create_publisher(
            PoseWithCovarianceStamped, '/initialpose', 10)

    def set_initial_pose(self, x, y, theta):
        """设置机器人初始位姿"""
        pose = PoseWithCovarianceStamped()
        pose.header.frame_id = 'map'
        pose.pose.pose.position.x = x
        pose.pose.pose.position.y = y
        pose.pose.pose.orientation.z = np.sin(theta/2)
        pose.pose.pose.orientation.w = np.cos(theta/2)
        
        with self.lock:
            self.initial_pose_pub.publish(pose)
            self.node.get_logger().info(f'Set initial pose: {x}, {y}, {theta}')

    async def navigate_to(self, x, y, theta):
        """导航到单个目标点"""
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.orientation.z = np.sin(theta/2)
        goal_msg.pose.pose.orientation.w = np.cos(theta/2)
        
        with self.lock:
            await self.nav_to_pose_client.wait_for_server()
            send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg)
            rclpy.spin_until_future_complete(self.node, send_goal_future)
            self.node.get_logger().info(f'Navigating to: {x}, {y}, {theta}')

    async def follow_waypoints(self, waypoints):
        """执行多点巡航"""
        goal_msg = FollowWaypoints.Goal()
        for wp in waypoints:
            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.pose.position.x = wp['x']
            pose.pose.position.y = wp['y']
            pose.pose.orientation.w = 1.0  # 默认朝向
            goal_msg.poses.append(pose)
        
        with self.lock:
            await self.follow_waypoints_client.wait_for_server()
            send_goal_future = self.follow_waypoints_client.send_goal_async(goal_msg)
            rclpy.spin_until_future_complete(self.node, send_goal_future)
            self.node.get_logger().info(f'Executing {len(waypoints)} waypoints')

    def cancel_navigation(self):
        """取消当前导航任务"""
        with self.lock:
            if self.nav_to_pose_client:
                self.nav_to_pose_client._cancel_goal_async()
            if self.follow_waypoints_client:
                self.follow_waypoints_client._cancel_goal_async()
            self.node.get_logger().info('Navigation cancelled')